/*
 * service.c
 */
#include <agl.h>
#include <common.h>
#include "spec.h"
#include "service.h"
#include "vardecl.h"
#include "psgdef.h"
#include "math.h"

#ifndef LOCAL
#define LOCAL
#endif

#define another(x) ((x == PLAYER1) ? PLAYER2 : PLAYER1)
#define FIELD_MIN (-GroundEdgeLoc)
#define FIELD_MAX (GroundEdgeLoc)

/*
========================================
  前進
========================================
*/
void
S_Forward(int which)
{
  register machine *mp;
  float x, y;
  float acc;
  int vel;

  mp = &Machine[which];
  x = mp->vector.x;
  y = mp->vector.y;
  vel = GETA_RAD * sqrt(x * x + y * y);
  if (vel >= TABLESIZ)		/* *** */
    vel = TABLESIZ - 1;
  acc = mp->type->AccTable[vel];
  mp->vector.x += acc * sintbl[mp->direct];
  mp->vector.y -= acc * costbl[mp->direct];

  /* 速度再計算 */
  x = mp->vector.x;
  y = mp->vector.y;
  vel = GETA_RAD * sqrt(x * x + y * y);
  switch (mp->fbflag) {
  case FB_STOP:
    mp->fbflag = FB_FORWARD;
    break;
  case FB_FORWARD:
    break;
  case FB_BACKWARD:
    if (vel == 0) {
      mp->vector.x = mp->vector.y = 0.0;
      mp->fbflag = FB_STOP;
    }
    break;
  }
}

/*
========================================
  後退
========================================
*/
void
S_Backward(int which)
{
  register machine *mp;
  float     x, y;
  float     acc;
  int       vel;
  int       NegFlagX,NegFlagY;
  int       NegFlagX2,NegFlagY2;

  mp = &Machine[which];
  
  x = mp->vector.x;
  y = mp->vector.y;
  /* 現在のベクターの符号を拾う */
  if (x < 0)
    NegFlagX = -1;
  else
    NegFlagX = 1;
  if (y < 0)
    NegFlagY = -1;
  else
    NegFlagY = 1;

  vel = GETA_RAD * sqrt(x * x + y * y);
  if (vel >= TABLESIZ)		/*  */
    vel = TABLESIZ - 1;
  acc = mp->type->BrakeTable[vel];
  mp->vector.x -= acc * sintbl[mp->direct] * mp->fbflag;
  mp->vector.y += acc * costbl[mp->direct] * mp->fbflag;

  /* 速度再計算 */
  x = mp->vector.x;
  y = mp->vector.y;
  /* 減速後のベクターの符号を拾う */
  if (x < 0)
    NegFlagX2 = -1;
  else
    NegFlagX2 = 1;
  if (y < 0)
    NegFlagY2 = -1;
  else
    NegFlagY2 = 1;
  /* もし、減速後にベクターの符号が反転しているようなら */
  /* バックしようとしているので速度を０として扱う */
  if(NegFlagX != NegFlagX2 || NegFlagY != NegFlagY2){
    vel = 0;
  } else {
    vel = GETA_RAD * sqrt(x * x + y * y);
  }
  switch (mp->fbflag) {
  case FB_STOP:
//    mp->fbflag = FB_BACKWARD;
    break;
  case FB_BACKWARD:
  case FB_FORWARD:
    if (vel == 0) {
      mp->vector.x = mp->vector.y = 0.0;
      mp->fbflag = FB_STOP;
    }
    break;
  }
}

/*
========================================
  右に曲がる
========================================
*/
void
S_TurnRight(int which)
{
  register machine *mp;
  int angle;
  float x, y, t;

  mp = &Machine[which];
  /* 停止中ならハンドルは切らない */
  if (mp->fbflag == FB_STOP)
    return;

  angle = mp->type->Handling * mp->fbflag;

  mp->direct -= angle;
  if (mp->direct < 0)
    mp->direct += 3600;
  mp->direct %= 3600;

  x = mp->vector.x;
  y = mp->vector.y;
  if (x > MAX_RAD)
    x = MAX_RAD;
  if (y > MAX_RAD)
    y = MAX_RAD;
  t = sqrt(x * x + y * y);
  mp->vector.x = t * sintbl[mp->direct] * mp->fbflag;
  mp->vector.y = -t * costbl[mp->direct] * mp->fbflag;
}

/*
========================================
  左に曲がる
========================================
*/
void
S_TurnLeft(int which)
{
  register machine *mp;
  int angle;
  float x, y, t;
  direct_t dir;
  short fb;

  mp = &Machine[which];
  /* 停止中ならハンドルは切らない */
  if (mp->fbflag == FB_STOP)
    return;

  angle = mp->type->Handling * mp->fbflag;

  mp->direct += angle;
  mp->direct %= 3600;

  x = mp->vector.x;
  y = mp->vector.y;
  if (x > MAX_RAD)
    x = MAX_RAD;
  if (y > MAX_RAD)
    y = MAX_RAD;
  t = sqrt(x * x + y * y);
  mp->vector.x = t * sintbl[mp->direct] * mp->fbflag;
  mp->vector.y = -t * costbl[mp->direct] * mp->fbflag;
}

/*
========================================
  現在の速度を返す
========================================
*/
accel_t
S_GetSpeed(int which)
{
  register machine *mp;

  mp = &Machine[which];
  return sqrt(mp->vector.x * mp->vector.x + mp->vector.y * mp->vector.y)
            * mp->fbflag;
}

/*
----------------------------------------
  ある角度に対しての端までの距離
----------------------------------------
*/
LOCAL distance
__CheckDist(direct_t angle,float x, float y)
{
  float l1,l2;

  /* 範囲外なら補正 */
  angle %= 3600;
  /* ９０度単位の角度の場合引き算で十分なので特別処理 */
  if( angle == 0 || angle == 900 || angle == 1800 || angle == 2700 ){
    switch(angle){
      case 0:
        l1 = FIELD_MIN - y;
        break;
      case 900:
        l1 = FIELD_MAX - x;
        break;
      case 1800:
        l1 = FIELD_MAX - y;
        break;
      case 2700:
        l1 = FIELD_MIN - x;
        break;
    }
  }
  else{
    l1 = -y;
    y = x;
    x = l1;
    if(angle > 0 && angle < 1800){
      if(angle < 900){
        l1= (FIELD_MAX - x) / costbl[angle];
        l2= (FIELD_MAX - y) / sintbl[angle];
      }
      else{
        l1= (FIELD_MIN - x) / costbl[angle];
        l2= (FIELD_MAX - y) / sintbl[angle];
      }
    }
    else{
      if(angle < 2700){
        l1= (FIELD_MIN - x) / costbl[angle];
        l2= (FIELD_MIN - y) / sintbl[angle];
      }
      else{
        l1= (FIELD_MAX - x) / costbl[angle];
        l2= (FIELD_MIN - y) / sintbl[angle];
      }
    }
    l1 = abs(l1);
    l2 = abs(l2);
    if(l1>l2){
      l1 = l2;
    }
  }
  return abs(l1);
}

/*
========================================
進行方向にあるフィールドの端までの距離を求める
========================================
*/
QuadDist *
S_CheckDist(int which)
{
  register machine *mp;
  distance dDist;
  direct_t tDirect;
  static QuadDist qQuadDist;
  float x, y;

  mp = &Machine[which];
  tDirect = mp->direct;
  x = Locate[which][LX];
  y = Locate[which][LY];
  /* 前 */
  qQuadDist.dFrontDist = __CheckDist(tDirect,x,y);
  /* 後 ( 180°回転 ) */
  qQuadDist.dBackDist  = __CheckDist(tDirect + 1800,x,y);
  /* 左 (  90°回転 ) */
  qQuadDist.dLeftDist  = __CheckDist(tDirect +  900,x,y);
  /* 右 ( 270°回転 ) */
  qQuadDist.dRightDist = __CheckDist(tDirect + 2700,x,y);

  return &qQuadDist;
}

/*
========================================
  自分のＸ座標を求める
========================================
*/
float
S_CheckX(int which)
{
  return Locate[which][LX];
}

/*
========================================
  自分のＹ座標を求める
========================================
*/
float
S_CheckY(int which)
{
  return Locate[which][LY];
}

/*
========================================
  自分の角度を求める
========================================
*/
direct_t
S_CheckDir(int which)
{
  return Machine[which].direct;
}

/*
========================================
  相手との距離を求める
  相手が、水平180°より後ろにいた場合
  距離は、-1になる。
========================================
*/
distance
S_CalcEnemyDist(int which)
{
  float myX,myY;
  float anX,anY;
  direct_t tEnemyDir;
  
  myX = Locate[which][LX];
  myY = Locate[which][LY];
  anX = Locate[another(which)][LX];
  anY = Locate[another(which)][LY];
  tEnemyDir = PointMakeDir(myX,myY,anX,anY);
  tEnemyDir -= S_CheckDir(which);
  if (tEnemyDir < 0)
    tEnemyDir += 3600;
  if(tEnemyDir > 900 && tEnemyDir < 2700){
    return (distance) -1;
  }
  myX -= anX;
  myY -= anY;
  return sqrt(myX * myX + myY * myY);
}

/*
========================================
  相手と自分との角度を求める
  相手が自分より後ろにいた場合
  角度は、-1になる。
========================================
*/
direct_t
S_SearchEnemy(int which)
{
  float myX,myY;
  float anX,anY;
  direct_t tEnemyDir;
  
  myX = Locate[which][LX];
  myY = Locate[which][LY];
  anX = Locate[another(which)][LX];
  anY = Locate[another(which)][LY];
  tEnemyDir = PointMakeDir(myX,myY,anX,anY);
  tEnemyDir -= S_CheckDir(which);
  if(tEnemyDir < 0)
    tEnemyDir += 3600;
  if(tEnemyDir > 900 && tEnemyDir < 2700){
  	return (direct_t) -1;
  }
  return tEnemyDir;
}
